#include "ConjuGradient.h"

//vector<double>* ConjuGradientOpt::g_pDose = NULL;
OptData* ConjuGradientOpt::g_optData = NULL;
vector<SConstraint>* ConjuGradientOpt::g_pConstraints = NULL;
vector<double>* ConjuGradientOpt::g_objFunValue=NULL;

ConjuGradientOpt* ConjuGradientOpt::Instance()
{

	static ConjuGradientOpt instance;
	return &instance;
}


void ConjuGradientOpt::function_grad(const real_1d_array &x, double &func, real_1d_array &grad, void *ptr)
{
	//long step1, step2, step3, step4,step5, step6, step7, step8;
	//step1 = clock();

	func = 0;
	for(int i=0; i!= x.length();i++)
	{
		grad[i] = 0;
	}
	//step2 = clock();
	vector<double> *tempWeight = new vector<double>(x.length());
	for(int indexResult = 0; indexResult!=x.length();indexResult++)
	{
		(*tempWeight)[indexResult] = x[indexResult]*x[indexResult];
	}
	//step3=clock();
	ConjuGradientOpt::g_pDose = new vector<double>;
	*ConjuGradientOpt::g_pDose = ConjuGradientOpt::g_optData->calculateDose(tempWeight);
	//step4=clock();
	vector<SConstraint>::iterator iterConstraint;
	for(iterConstraint = ConjuGradientOpt::g_pConstraints->begin();iterConstraint != ConjuGradientOpt::g_pConstraints->end();iterConstraint++)
	{
		//step5 = clock();
		//initialization
		string strStructureName = iterConstraint->strStructureName;
		double aimDose = iterConstraint->aimDose;
		double aimWeight = iterConstraint->aimWeight;
		string constraintType = iterConstraint->constraintType;
		vector<vector<double> > *vvDij = ConjuGradientOpt::g_optData->getDijTotal();

		vector<int> *aimStructure=NULL;
		for(int indexStr = 0; indexStr != ConjuGradientOpt::g_optData->getStructures()->size(); indexStr++)
		{
			if((*ConjuGradientOpt::g_optData->getStructures())[indexStr].getStructName() == strStructureName)
			{
				aimStructure = (*ConjuGradientOpt::g_optData->getStructures())[indexStr].getPoints();
			}
		}
		if(aimStructure==NULL)
		{
			cerr<<"structure error"<<endl;
		}
		//step6 = clock();
		//create func and grad[]
		double tempTotleFunc = 0;
		for(int indexPoint = 0; indexPoint!=aimStructure->size();indexPoint++)
		{
			double temp = 0;
			double contri = 0;
			double wei =0;
			unsigned int position = (*aimStructure)[indexPoint];

			temp = (*g_pDose)[position];

			if((constraintType == "upper")&&(temp>aimDose)){
				tempTotleFunc += (temp - aimDose)*(temp - aimDose);
			}else if((constraintType == "lower")&&(temp<aimDose)){
				tempTotleFunc += (temp - aimDose)*(temp - aimDose);
			}else if((constraintType == "uniform")){
				tempTotleFunc += (temp - aimDose)*(temp - aimDose);
			}
		} 
		func += (tempTotleFunc/double(aimStructure->size()))*aimWeight;

		//step7=clock();
		//calculate grad[i]
		for(unsigned int j=0;j!=x.length();j++)
		{
			double temp = 0;
			double temp2 = 0;
			double tempTotalGrad = 0;

			for(int indexPoint = 0; indexPoint!=aimStructure->size();indexPoint++)
			{
				temp = 0;
				temp2 = 0;
				double contri = 0;
				double wei =0;
				unsigned int position =(*aimStructure)[indexPoint];

				if((*vvDij)[position].size()>j){
					contri = (*vvDij)[position][j];
				}

				if(contri > 0.0000000001)
				{
					temp2 =  contri * x[j];

					temp = (*g_pDose)[position];

					if((constraintType == "upper")&&(temp>aimDose)){
						tempTotalGrad += 4*(temp - aimDose)*temp2;
					}else if((constraintType == "lower")&&(temp<aimDose)){
						tempTotalGrad += 4*(temp - aimDose)*temp2;
					}else if((constraintType == "uniform")){
						tempTotalGrad += 4*(temp - aimDose)*temp2;
					}
				}
			}
			grad[j] += (tempTotalGrad/double(aimStructure->size()))*aimWeight;

		}
		//step8=clock();
	}

	cout<<func<<endl;
	g_objFunValue->push_back(func);
	//cout<<step2-step1<<"\t"<<step3-step2<<"\t"<<step4-step3<<"\t"<<step5-step4<<"\t"<<step6-step5<<"\t"<<step7-step6<<"\t"<<step8-step7<<endl;
	delete ConjuGradientOpt::g_pDose;
}

void ConjuGradientOpt::optimization(vector<double> *resutlWeight,OptData* optData, vector<SConstraint> *pConstrain, vector<double> *objFunValue,int maxIter)
{
	ConjuGradientOpt::g_optData = optData;
	ConjuGradientOpt::g_pConstraints = pConstrain;
	ConjuGradientOpt::g_objFunValue = objFunValue;

	unsigned int nWeightLen = ConjuGradientOpt::g_optData->getWeightTotal()->size();   
	//size_t xLen = g_pDoseMatrix->m_vWeightUni->size();
	//initialize weight
	//make sure all beamlets used in optimization are != 0
	//since if beamlet weight == 0, it will always == 0 

	double *xArray = new double[nWeightLen];
	for(int indexWei = 0; indexWei != nWeightLen; indexWei++)
	{
		xArray[indexWei] = sqrt((*ConjuGradientOpt::g_optData->getWeightTotal())[indexWei]);
	}

	real_1d_array x;
	x.setcontent(nWeightLen,xArray);
	delete xArray;

	double epsg = 0.0000000001;
	double epsf = 0.00000;
	double epsx = 0.00000;
	ae_int_t maxits = maxIter;
	mincgstate state;
	mincgreport rep;

	mincgcreate(x, state);
	mincgsetcond(state, epsg, epsf, epsx, maxits);

	alglib::mincgoptimize(state,ConjuGradientOpt::function_grad);

	mincgresults(state, x, rep);

	for(int i=0;i<x.length();i++)
	{
		resutlWeight->push_back(x[i]*x[i]);
	}    
}

void ConjuGradientOpt::function_grad_modify_1(const real_1d_array &x, double &func, real_1d_array &grad, void *ptr)
{
	//long step1, step2, step3, step4,step5, step6, step7, step8;
	//step1 = clock();

	func = 0;
	for(int i=0; i!= x.length();i++)
	{
		grad[i] = 0;
	}
	//step2 = clock();
	vector<double> *tempWeight = new vector<double>(x.length());
	for(int indexResult = 0; indexResult!=x.length();indexResult++)
	{
		(*tempWeight)[indexResult] = x[indexResult]*x[indexResult];
	}
	//step3=clock();
	ConjuGradientOpt::g_pDose = new vector<double>;
	*ConjuGradientOpt::g_pDose = ConjuGradientOpt::g_optData->calculateDose(tempWeight);
	//step4=clock();
	vector<SConstraint>::iterator iterConstraint;
	for(iterConstraint = ConjuGradientOpt::g_pConstraints->begin();iterConstraint != ConjuGradientOpt::g_pConstraints->end();iterConstraint++)
	{
		//step5 = clock();
		//initialization
		string strStructureName = iterConstraint->strStructureName;
		double aimDose = iterConstraint->aimDose;
		double aimWeight = iterConstraint->aimWeight;
		string constraintType = iterConstraint->constraintType;
		vector<vector<double> > *vvDij = ConjuGradientOpt::g_optData->getDijTotal();

		vector<int> *aimStructure=NULL;
		for(int indexStr = 0; indexStr != ConjuGradientOpt::g_optData->getStructures()->size(); indexStr++)
		{
			if((*ConjuGradientOpt::g_optData->getStructures())[indexStr].getStructName() == strStructureName)
			{
				aimStructure = (*ConjuGradientOpt::g_optData->getStructures())[indexStr].getPoints();
			}
		}
		if(aimStructure==NULL)
		{
			cerr<<"structure error"<<endl;
		}
		//step6 = clock();
		//create func and grad[]
		double tempTotleFunc = 0;
		for(int indexPoint = 0; indexPoint!=aimStructure->size();indexPoint++)
		{
			double temp = 0;
			double contri = 0;
			double wei =0;
			unsigned int position = (*aimStructure)[indexPoint];

			temp = (*g_pDose)[position];

			if((constraintType == "upper")&&(temp>aimDose)){
				tempTotleFunc += (temp - aimDose)*(temp - aimDose);
			}else if((constraintType == "lower")&&(temp<aimDose)){
				tempTotleFunc += (temp - aimDose)*(temp - aimDose);
			}else if((constraintType == "uniform")){
				tempTotleFunc += (temp - aimDose)*(temp - aimDose);
			}
		} 
		func += (tempTotleFunc/double(aimStructure->size()))*aimWeight;

		//step7=clock();
		//calculate grad[i]
		for(unsigned int j=0;j!=x.length();j++)
		{
			double temp = 0;
			double temp2 = 0;
			double tempTotalGrad = 0;

			for(int indexPoint = 0; indexPoint!=aimStructure->size();indexPoint++)
			{
				temp = 0;
				temp2 = 0;
				double contri = 0;
				double wei =0;
				unsigned int position =(*aimStructure)[indexPoint];

				if((*vvDij)[position].size()>j){
					contri = (*vvDij)[position][j];
				}

				if(contri > 0.0000000001)
				{
					temp2 =  contri * x[j];

					temp = (*g_pDose)[position];

					if((constraintType == "upper")&&(temp>aimDose)){
						tempTotalGrad += 4*(temp - aimDose)*temp2;
					}else if((constraintType == "lower")&&(temp<aimDose)){
						tempTotalGrad += 4*(temp - aimDose)*temp2;
					}else if((constraintType == "uniform")){
						tempTotalGrad += 4*(temp - aimDose)*temp2;
					}
				}
			}
			grad[j] += (tempTotalGrad/double(aimStructure->size()))*aimWeight;

		}
		//step8=clock();
	}

	cout<<func<<endl;
	g_objFunValue->push_back(func);
	//cout<<step2-step1<<"\t"<<step3-step2<<"\t"<<step4-step3<<"\t"<<step5-step4<<"\t"<<step6-step5<<"\t"<<step7-step6<<"\t"<<step8-step7<<endl;
	delete ConjuGradientOpt::g_pDose;
}

void ConjuGradientOpt::optimization_modify_1(vector<double> *resutlWeight,OptData* optData, vector<SConstraint> *pConstrain, vector<double> *objFunValue,int maxIter)
{
	ConjuGradientOpt::g_optData = optData;
	ConjuGradientOpt::g_pConstraints = pConstrain;
	ConjuGradientOpt::g_objFunValue = objFunValue;

	unsigned int nWeightLen = ConjuGradientOpt::g_optData->getWeightTotal()->size();   
	//size_t xLen = g_pDoseMatrix->m_vWeightUni->size();
	//initialize weight
	//make sure all beamlets used in optimization are != 0
	//since if beamlet weight == 0, it will always == 0 
	vector<double> tempWeight;
	tempWeight.resize(nWeightLen,0);
	for(int i=0; i<optData->getDijTotal()->size(); i++){
		for(int j=0;j<(*optData->getDijTotal())[i].size();j++){
			if((*optData->getDijTotal())[i][j]>0){
				tempWeight[j]+=(*optData->getDijTotal())[i][j];
			}
		}
	}


	double *xArray = new double[nWeightLen];
	for(int indexWei = 0; indexWei != nWeightLen; indexWei++)
	{
		if((*ConjuGradientOpt::g_optData->getWeightTotal())[indexWei]>0){
			xArray[indexWei] = sqrt((*ConjuGradientOpt::g_optData->getWeightTotal())[indexWei]);
		}else if(tempWeight[indexWei]>0){
			xArray[indexWei] = 0.00001;
		}

	}

	real_1d_array x;
	x.setcontent(nWeightLen,xArray);
	delete xArray;

	double epsg = 0.0000000001;
	double epsf = 0.00000;
	double epsx = 0.00000;
	ae_int_t maxits = maxIter;
	mincgstate state;
	mincgreport rep;

	mincgcreate(x, state);
	mincgsetcond(state, epsg, epsf, epsx, maxits);

	alglib::mincgoptimize(state,ConjuGradientOpt::function_grad);

	mincgresults(state, x, rep);

	for(int i=0;i<x.length();i++)
	{
		resutlWeight->push_back(x[i]*x[i]);
	}    
}